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ABSTRACT 


The  purpose  of  this  thesis  is  to  investigate  the  application  of  a  six-state  discrete 
Kalman  filter  for  estimates  of  angular  rates  based  solely  on  star  sensor  data.  The  satellite 
is  in  a  Molnyia  orbit  where  orbital  angular  velocity  and  orbital  angular  acceleration  are 
predetermined  and  stored  in  the  on-board  computer;  such  that  they  will  be  available  each 
time  a  star  observation  is  made.  A  two-axis  star  sensor  will  provide  two  angles  to  the 
estimator  whereupon  the  third  "unsensed"  angle  will  be  predicted;  the  rates  about  all  three 
axes  are  then  estimated.  The  results  show  that  the  rate  estimates  are  accurate  to  within 
10'^  't/s,  which  is  equivalent  to  the  data  produced  by  gyroscopes. 
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I. 


INTRODUCTION 


The  space  industry  has  grown  at  an  alarming  rate  over  the  last  decade  and  should 
continue  to  do  so  in  the  future.  Information  and  data  received  from  satellites  is  generally 
taken  for  granted.  Only  when  a  critical  satellite  outage  occurs  is  society  rudely  reminded 
of  their  ever-increasing  dependence  on  these  vehicles.  For  example,  the  premature  and 
unexpected  failure  of  Galaxy  IV  temporarily  left  millions  of  people  without  pager 
service.  This  dependence  translates  into  big  business.  Therefore,  when  a  satellite  suffers 
a  failure  that  threatens  its  life  expectancy,  every  effort  will  be  made  to  save  it. 

Recently,  the  SOHO  spacecraft  tumbled  out  of  control  after  suffering  from 
multiple  gyroscope  failures.  As  a  result,  European  engineers  were  forced  to  create  a 
software  package  that  would  over-ride  the  failed  hardware.  It  took  six  months  to  write 
and  test  the  code,  but  it  was  time  well  spent,  as  control  of  the  billion  dollar  spacecraft  was 
once  again  regained  after  a  successful  up-link.  SOHO  is  now  able  to  autonomously 
maintain  proper  attitude  relative  to  the  sun  using  its  star  tracker  as  the  primary  control 
sensor.  As  a  matter  of  fact,  this  may  be  a  sign  of  things  to  come  considering  that  the 
reliability  of  a  gyroscope  decreases  significantly  with  time.  Even  though  the  addition  of 
redundant  gyroscopes  will  serve  to  increase  reliability,  it  will  also  increase  cost, 
complexity  and  mass.  SOHO  is  testimony  to  the  fact  that,  in  certain  cases,  software  is 
more  feasible  than  hardware. 

Manufacturers  of  gyroscopes  will  obviously  be  opposed  to  the  demise  of  their 
hardware,  but  with  improvements  in  both  on-board  processing  and  star  sensor 
capabilities,  this  area  of  investigation  can  no  longer  be  ignored.  To  this  end,  the  purpose 
of  this  thesis  is  to  determine  the  utility  of  a  six-state,  discrete  Kalman  filter  in  the 
estimation  of  satellite  attitude. 

A.  OVERVIEW 

In  order  to  test  the  Kalman  filter,  an  attitude  control  system  must  be  developed. 

To  help  in  this  development,  certain  design  criteria  were  mandated  while  others  were 
self-imposed.  Chapter  II  will  summarize  these  requirements  and  assumptions.  The 
proposed  control  flow  of  the  attitude  control  system  is  shown  below  in  Figure  1 . 
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Figure  1 :  Attitude  Control  Diagram 


Disturbance  moments  will  force  the  vehicle  dynamics  and  will  be  briefly  discussed  in 
Chapter  III;  in  addition,  the  orbital  equations  of  motion  will  also  be  given  in  this  chapter. 
The  spacecraft  equations  of  motion  will  be  the  topic  of  Chapter  IV.  The  simulation  will 
progress  at  discrete  ten  second  intervals,  which  will  be  the  sampling  time  of  each  star 
sensor  to  be  described  in  Chapter  V.  Once  a  star  sensor  makes  a  noisy  measurement,  that 
information  will  be  sent  to  the  discrete  Kalman  filter  where  both  attitude  and  attitude 
rates  will  be  estimated;  Chapter  VI  will  describe  this  process  in  more  detail.  The 
estimated  output  of  the  filter  will  be  the  input  to  the  proportional  plus  derivative 
controller  outlined  in  Chapter  VII.  The  results  and  conclusion  will  follow  accordingly. 
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II.  CONTROL  SYSTEM  DESIGN  SPECIFICATIONS 


As  stated  in  the  introduction,  the  purpose  of  this  thesis  is  to  develop  a  suitable 
attitude  estimator  based  on  star  sensor  measurements.  The  attitude  control  system  will  be 
designed  according  to  the  following  parameters. 

A.  SATELLITE  SPECIFICATIONS 

•  Molnyia  orbit 

•  Three  star  sensors  aligned  with  the  body  axes 

•  ~10  second  star  sensor  sampling  time 

•  1  star  present  in  star  sensor  field  of  view  (FOV)  at  all  times 

•  1  star  sensor  chosen  at  random  at  each  time  step 

•  Roll  and  pitch  inertia=25,000  IcgW,  yaw  inertia=l 5,000  kg-m^ 

•  Kalman  filter  for  rate  estimation 

.  B.  SELF-IMPOSED  SPECIFICATIONS 

•  Nadir  pointing  to  within  0. 1  °  of  orbital  reference  fi-ame 

•  4  arc-second  noise  level  for  each  star  sensor 

•  3 -axis  stabilized 

•  10-year  design  life 

C.  ASSUMPTIONS 

•  Small  angle  approximations 

•  Orbital  angular  velocity  and  acceleration  known  for  each  sensor  measurement 

•  Negligible  cross  products  of  inertia 

•  Constant  solar  pressure  moments 

•  Each  reaction  wheel  is  independently  controlled 

•  No  slewing  requirement 

•  Control  law  updates  performed  at  10  second  intervals 

•  Satellite  is  modeled  as  a  rigid  body 
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D.  CONTROL  SYSTEM  DESIGN  CONSIDERATIONS 

As  can  be  seen  from  the  previous  sections,  considerable  latitude  has  been  given  in 
the  design  of  this  control  system.  In  order  to  achieve  0.1°  pointing  accuracy,  a  zero- 
momentum  system,  consisting  of  three  reaction  wheels  whose  momentum  vectors 
coincide  with  the  body  axes,  will  be  used.  These  reaction  wheels  will  each  be 
independently  controlled  with  its  own,  dedicated  proportional  plus  derivative  (PD) 
controller.  Control  of  the  satellite  will  be  difficult  at  perigee  due  to  its  high  orbital 
angular  velocity;  consequently,  the  gains  of  the  pitch  controller  will  have  to  be  adjusted 
accordingly.  As  disturbance  moments  cause  errors  in  attitude,  off-axis  components  of 
reaction  wheel  angular  momentum  will  cause  internal  torques  that  will  have  to  be 
accounted  for.  The  star  sensor  described  in  Section  A  of  this  chapter  is  only  able  to  sense 
errors  about  two  axes,  which  means  that  the  Kalman  filter  will  have  to  predict  the  error 
about  the  "un-sensed"  axis.  Not  only  will  the  filter  be  used  to  estimate  position  and  rates,  . 
but  it  will  also  mitigate  sensor  noise.  Cracial  to  this  design  is  the  assumption  that  the 
orbital  angular  velocity  and  acceleration  are  known  for  each  star  tracker  observation. 
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III.  SPACE  ENVIRONMENT 


A.  MOLNYIA  ORBIT 

Countries  located  in  high  latitudes  are  forced  to  place  communication  satellites  in 
highly  eccentric  orbits  known  as  Molnyia  orbits.  These  orbits,  widely  used  by  the  Soviet 
Union,  have  the  following  characteristics:  63.5°  inclination,  elliptical,  1 1-12  hour  period. 
Due  to  the  high  eccentricity  of  this  orbit,  the  spacecraft  has  a  long  dwell  time  over  the 
area  of  interest. 


The  inclination  of  this  orbit  is  chosen  such  that  perigee  will  remain  fixed  over  Antarctica. 
In  order  to  provide  continuous  coverage,  at  least  three  satellites  need  to  be  appropriately 
phased.  Table  1  lists  the  characteristics  of  this  orbit. 


Title 

Symbol/Equation 

Quantity 

Earth's  Gravitational  Constant 

398601  kmVsec^ 

Inclination 

i 

63.5° 

Radius  of  Perigee 

7378.15  km 

Radius  of  Apogee 

ra 

42164.17  km 

5 


Semi-Major  Axis 

r  +  r 

^  'a 

a  = - 

2 

24771.16  km 

Eccentricity 

1 

II 

0.70215 

+r^ 

semi-latus  rectum 

1 

ii 

12558.62  km 

period 

38799.86  sec 

Table  1 :  Orbit  Parameters 


Of  particular  interest  is  the  orbital  angular  velocity  of  the  spacecraft  at  any  time,  t.  These 
values  will  be  calculated  in  discrete,  ten-second  intervals  and  stored  as  deterministic 
values  in  the  on-board  computer.  The  radius,  as  a  function  of  time,  is  given  by 


r(t)  = 


P 

\  +  e  cos(vft)) 


(1) 


The  true  anomaly,  v ,  is  just  the  angle  measured  from  perigee  to  the  satellite's  current 
position.  Taking  both  the  first  and  second  derivatives  [Ref  1],  the  following  expressions 
are  obtained 


r(0  = 


^esin(vft)) 

\P 


(2) 


r(0  =  J—^(^os(y(t))v(t) 

Ip 


(3) 


The  rate  of  change  of  true  anomaly,  v ,  is  just  the  orbit  angular  rate  of  the  satellite.  Both 
the  orbital  angular  rate  and  the  orbital  angular  acceleration  can  be  found  in  [Ref  1]. 
They  are  given  by 


v(0  =  J—  (l  +  e  cos(v(0) 

V  P  ^(0 


(4) 


sin(v^(?))(l  +  e  cos(v'(/)))+  re  sin(v(/))i>(/) 


(5) 


In  order  to  solve  for  both  the  true  anomaly  and  radius  at  any  time,  t,  it  is  necessary  to 
convert  the  above  equations  into  two  first  order  differential  equations.  If  the  following 
variable  substitutions  are  made:  y^=r ,  y^=r ,  y^=v  ,  then  they  can  be 

substituted  in  Equations  (1)  through  (5).  Once  the  substitution  is  made,  the  resulting  first 
order  differential  equations  can  be  integrated  using  a  Runge-Kutta  integration  method. 
The  integration  was  performed  using  MATLAB  5.2,  and  the  results  are  shown  in  Figure 
3. 


The  simulation  begins  at  perigee  and  continues  for  an  entire  orbit.  As  can  be  seen,  the 
orbital  angular  velocity  is  nearly  constant  as  the  spacecraft  dwells  near  apogee. 


7 


B.  DISTURBANCE  TORQUES 


At  first  glance,  it  may  seem  that  the  vacuum  of  space  is  a  benign  environment. 
However,  this  is  not  true.  External  disturbance  moments  will  cause  errors  in  the 
spacecraft's  attitude.  These  errors  however,  will  be  kept  within  the  required  pointing 
limits  if  the  attitude  control  system  is  properly  designed.  The  four  major  disturbance 
moments  worth  consideration  are  1)  Solar  Pressure  2)  Gravity  Gradient  3)  Magnetic 
Moment  and  4)  Aerodynamic.  While  the  last  three  disturbance  moments  are  significant 
at  perigee,  they  are  insignificant  at  apogee.  Since  the  satellite  will  spend  the  majority  of 
its  time  at  or  near  apogee,  the  disturbance  torques  will  be  modeled,  for  this  case.  As  a 
consequence,  both  magnetic  and  aerodynamic  moments  will  be  discounted.  For  design 
simplicity,  it  will  be  assumed  that  the  solar  pressure  moment  can  be  modeled  as  a 
constant  torque  about  each  body  axis.  Although  gravity  gradient  moments  are  relatively 
insignificant  at  apogee,  they  can  be  incorporated  into  the  satellite  equations  of  motion 
with  minimal  effort.  These  moments,  derived  in  Appendix  A,  were  found  to  be 


(6) 


Since  I,  is  less  than  both  4  and  ly ,  this  satellite  will  be  gravity  gradient 
friendly.  This  could  become  important  during  safe  mode  operations.  Although  this 
spacecraft  is  gravity  gradient  friendly,  the  symmetry  about  the  roll  and  pitch  axes 
compromises  yaw  stability.  As  long  as  the  attitude  control  system  remains  operational, 
however,  yaw  stability  will  not  be  a  factor. 
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IV.  SATELLITE  DYNAMICS  AND  KINEMATICS 


There  are  many  types  of  transformation  methods,  the  most  popular  are:  direction 
cosine  matrices  (DCM),  euler  angles,  and  quaternions.  Quaternions  are  popular  since 
they  involve  only  a  single  rotation  about  an  eigen-axis.  This  greatly  reduces  the 
cumbersome  mathematics  that  is  characteristic  of  the  other  methods.  However,  if  small 
angle  approximations  are  made  and  if  second  order  terms  are  set  to  zero,  then  DCM's  are 
easy  to  employ;  they  will  be  used  in  this  analysis.  Transformations  from  one  frame  to 
another  are  performed  to  facilitate  calculations.  For  example,  the  latitude  and  longitude 
■  of  stars  in  the  star  catalog  have  all  been  programmed  within  a  celestial  frame,  but 
measurements  will  be  made  in  the  body  frame.  Therefore,  proper  attitude  determination 
relies  on  a  simple  transformation. 

Satellite  dynamics  refers  to  the  motion  of  the  body  when  subject  to  both  internal 
and  external  disturbance  moments.  The  assumption  has  been  made  that  the  spacecraft 
will  rotate  about  its  principal  moments  of  inertia.  This  is  a  reasonable  assumption  since 
the  off-axis  inertias  can  be  significantly  reduced  by  strategic  placement  of  satellite 
components  and  hardware. 

A.  REFERENCE  FRAMES 

In  the  field  of  attitude  control,  it  is  often  required  to  express  an  inertial  quantity  as 
a  body  frame  quantity.  For  example,  the  inertial  angular  velocity  derived  from  the  Euler 
moment  equations  must  be  expressed  in  body  coordinates  and  then  integrated  to  get  the 
Euler  angles.  Three  important  reference  frames  are  used  in  the  derivation  of  equations  of 
motion:  1)  body  frame  2)  orbital  frame  and  3)  inertial  frame.  The  origin  of  these  three 
frames  will  all  be  located  at  the  spacecraft's  center  of  mass.  In  the  orbital  reference 
frame,  the  z-axis  points  at  the  center  of  the  Earth,  the  x-axis  points  in  the  satellite's 
direction  of  motion,  and  the  y-axis  is  normal  to  the  orbit  plane,  completing  the  right-hand' 
set.  The  body  frame  is  attached  to  the  spacecraft  where  the  Euler  angles  represent  the 
deviation  of  the  body  frame  from  the  orbital  reference  frame.  On-board  sensors  measure 
these  Euler  angles.  The  inertial  frame  remains  fixed  in  Earth  space  such  that  the  inertial 
y-axis  coincides  with  the  orbital  y-axis.  An  additional  reference  frame  alluded  to  earlier 
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is  the  celestial  frame.  The  z-axis  of  this  frame  points  north  and  the  x-axis  points  in  the 
direction  of  the  vernal  equinox.  Although  the  x-axis  precesses  (very  slowly),  it  assumed 
to  be  fixed  in  space. 


B.  STATE  SPACE  EQUATIONS  OF  MOTION 

The  equations  of  motion  derived  in  Appendix  C  completely  describe  the  motion 
of  the  satellite  when  subjected  to  both  internal  and  external  disturbance  moments.  If  the 
body  accelerations  are  solved  for  in  Equation  (55)  the  following  result  is  obtained 
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For  computational  reasons,  it  is  desirable  to  reduce  these  second  order  equations  to  first 
order  equations  by  making  the  following  state  variable  substitutions 


x  =  [(l>  ^  0  6  y/  yrY 


(8) 


With  these  definitions,  we  can  express  the  satellite  dynamic  equations  into  the  following 
matrix  form 


X-  Ax  ^Bu 


(9) 


A  is  the  plant  matrix  and  it  is  given  by 


AA 


4 

0 

-3. 

4 

0 


1 

0 

0 

± 

4 

0 


0  0 
0 


0 

-arf(4-4) 


-04-/,+/,)^ 


I. 


4 

0 


0 

n 

0 

3 

4 

0 


0 


4 

4  4 


0 

Hn(-4,+4-4)-h4 

4 

0 

dk 

4 

1 


(10) 


B  is  the  control  matrix  given  by 


B  = 


0  0  0 
1  0  0 
0  0  0 
0  1  0 
0  0  0 
0  0  1 


(11) 
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u  is  going  to  be  the  input  reaction  wheel  control,  and  it  will  have  the  following  form 


u  =  + 


(12) 


F  will  be  the  PD  controller  gain  matrix  and  uj  will  represent  the  summation  of  the  solar 
pressure  moments  and  the  internal  reaction  wheel  moments.  F  and  are  given  by 


'^spx  +^Z 
^spy 

z 


(13)  • 


(14) 


Substituting  Equation  (14)  into  Equation  (9),  the  following  equation  of  motion  is 
obtained 


X  =  (A  —  BF)x+Buj 


(15) 


Equation  (15)  is  equivalent  to  the  equations  of  motion  derived  in  Appendix  C. 
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V. 


STAR  SENSOR 


Star  sensors  were  chosen  because  of  their  high  accuracy  and  because  of  their 
ability  to  determine  yaw.  Earth  horizon  sensors  can  sense  only  roll  and  pitch.  Even 
though  sun  sensors  can  measure  yaw,  four  or  more  would  be  required  for  continuous 
information;  fiirthermore,  they  become  ineffective  while  in  Earth's  umbra.  Historically, 
star  sensors  have  been  used  only  to  update  gyroscopic  drift  estimates,  but  recent 
advancements  in  star  sensor  technology  make  it  possible  to  consider  practical 
implementations  of  attitude  determination  based  solely  on  star  sensor  data  [Ref.  8]. 

A.  STAR  CHARACTERISTICS 

For  the  purpose  of  this  simulation,  it  will  be  assumed  that  there  is  a  uniform 
distribution  of  stars  in  the  celestial  sphere,  such  that  one  star  will  be  present  in  the  sensor 
FOV  at  all  times.  Each  star  can  be  classified  according  to  its  magnitude  and  its  spectra. 
The  magnitude  refers  to  a  star's  brightness  as  seen  from  Earth.  The  magnitude  of  Vega,  a 
bright  star  in  the  constellation  Lyra,  has  been  assigned  a  value  of  zero.  All  other  stars  are 
referenced  to  Vega  logarithmically.  As  an  example,  the  brightest  star  outside  our  solar 
system,  Sirius,  has  a  magnitude  of  -1.6.  Astronomers  can  distinguish  different  stars,  not 
only  from  their  magnitude,  but  also  from  their  unique  spectral  characteristics.  There  are 
seven  principal  spectral  categories:  0,  B,  A,  F,  G,  K,  and  M  and  these  categories  are 
further  subdivided  into  ten  more  subgroups,  from  0  to  9.  [Ref.  3] 

B.  HOW  A  STAR  SENSOR  WORKS 

There  are  three  important  phases  in  the  determination  of  attitude  using  star 
trackers  1)  star  identification  2)  tracking  3)  processing  [Ref  3].  When  a  satellite 
completes  an  orbit,  the  star  sensor  will  have  imaged  stars  that  make  up  a  ring  of  the 
celestial  sphere.  The  width  of  this  celestial  ring  is  a  function  of  the  sensor  FOV. 
Although  accuracy  will  increase  with  a  small  FOV,  the  time  between  observations  will 
increase.  Once  the  FOV  is  decided  upon,  the  celestial  latitudes  and  longitudes  of  chosen 
stars,  within  the  sensor  celestial  ring,  are  recorded  into  the  star  catalog.  Therefore,  at  any 
point  in  the  orbit,  the  star  sensor  will  expect  to  see  a  certain  star.  Identification  of  this 
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star  is  made  if  it  falls  within  a  circle  of  tolerance;  if  two  stars  appear  in  this  circle, 
magnitude  and  spectral  characteristics  will  separate  the  two.  Once  the  star  is  identified,  it 
is  tracked  until  it  leaves  the  FOV.  While  the  sensor  is  tracking  an  identified  star, 
however,  the  measured  latitude  and  longitude  is  compared  with  the  actual  latitude  and 
longitude.  This  error  is  sent,  via  the  Kalman  filter,  to  the  appropriate  controller  in  the 
form  of  a  voltage  signal,  where  a  corrective  torque  is  subsequently  applied. 

C.  STAR  SENSOR  CHARACTERISTICS 

The  star  sensor  model  used  in  this  simulation  has  been  designed  in  accordance 
with  the  specifications  outlined  in  Chapter  II.  Further  assumptions  have  been  made  for 
the  sake  of  completeness,  and  they  will  be,  in  part,  consistent  with  current  technology. 


Technology 

Charged  Coupled  Device  (CCD) 

FOV 

10°xl0° 

Accuracy 

~10  arcsec 

#  Stars  in  Catalog 

4000 

Sampling  Rate 

0.1  Hz  (current  technology  is  faster) 

Noise 

4  arcsec  (magnitude=6)  • 

Solar  Exclusion  Angle 

30°  w/sun  shade 

Table  2:  Star  Tracker  Characteristics 


The  noise  level  is  inherent  to  the  star  tracker  and  it  is  treated  as  a  zero-mean,  Gaussian 
white  sequence. 

D.  STAR  SENSOR  OPERATION 

Three-axis  attitude  determination  requires  two  separate  line  of  sights  (LOS)  with 
angular  separation  near  90°  for  increased  accuracy.  In  this  simulation,  the  optical  axis  of 
each  star  sensor  will  be  aligned  with  the  body  axes.  At  each  discrete  time  step  a  star 
sensor  will  be  selected  at  random  for  attitude  determination.  Although  this  sequence  of 
events  permits  only  one  LOS  per  time  step,  attitude  is  readily  determined  over 
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consecutive  time  steps.  Since  only  one  star  will  be  in  the  sensor  FOV  at  any  particular 
time,  measurements  can  only  be  made  about  two  axes.  Table  3  summarizes  the  angles 
that  can  be  sensed  by  each  star  sensor. 


Star  Sensor  (Direction  of  Optical  Axis) 

Angles  Sensed 

Roll 

Pitch,  Yaw 

Pitch 

Roll,  Yaw 

Yaw 

Roll,  Pitch 

Table  3:  Star  Sensor  Measurements 
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VI.  DISCRETE  KALMAN  FILTER 


A  six  state  discrete  Kalman  filter  has  been  chosen  to  estimate  both  position  and 
rates  from  noisy  star  sensor  data.  The  Kalman  filter  that  will  be  used  in  the  simulation  is 
represented  by 


(16) 


Zi  =Hx^  +Vi 


The  white  sequence,  ,  for  the  plant  has  a  covariance,  Q,  while  the  sensor  noise,  ,  has 
a  covariance,  R.  Noise  from  the  star  sensor  is  affected  by  the  magnitude  of  the  star;  a 
bright  star  is  noisier  than  a  dim  star  [Ref  4],  The  sensor  noise  cov^ance  is  defined  as 
follows 


(17) 

Table  4  is  a  summary  of  all  the  matrices  and  their  respective  dimensions  that  will  be  used 
in  the  estimation  process. 


Symbol 

Definition 

state  vector 

6x1 

state  transition  matrix 

deterministic  weighting  matrix 

6x3 

deterministic  forcing  fimction 

3x1 

n 

plant  white  noise  sequence 

6x1 

Qk 

plant  noise  covariance  matrix 

6x6 

^k 

measurement  vector 

3x1 

Hk 

feedback  sensitivity  matrix 

3x6 

^k 

sensor  white  noise  sequence 

3x1 
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Rk 

sensor  noise  covariance  matrix 

3x3 

Pk 

error  covariance  matrix,  E 

(accuracy  of  estimate) 

6x6 

optimal  estimate  of  at  time,  4 ,  based  on  current  measurement 

6x1 

estimate  of  at  time,  ,  just  prior  to  measurement 

6x1 

Table  4:  Kal 

[man  Filter  Definitions 

A.  DEMVATION  OF  THE  Q  MATRIX 

Solving  for  the  covariance  of  the  plant  noise  is  no  trivial  matter.  In  this 
simulation,  the  Q  matrix  will  vary  with  each  time  step.  The  formal  definition  of  the  plant 
noise  covariance  is  given  by 

It  can  be  shown  that  Equation  (18)  must  satisfy  the  following  matrix  differential  equation 
[Ref  5] 


Qk  =  (19) 

The  augmented  A  matrix  is  defined  from  Equation  (15)  as  the  quantity,  A-BF  and  the 
power  spectral  density  matrix  associated  vrith  the  forcing  function  u  is  denoted  by  W. 
The  solution  to  Equation  (19)  is  greatly  simplified  for  the  time  invariant  case.  It  proceeds 
as  follows 


a  = 


A/ 


(20) 


By  taking  the  matrix  exponential  of  Equation  (20),  the  following  result  is  obtained 
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^'[o 

The  upper  left  partition  can  be  neglected  for  this  analysis.  The  plant  noise  covariance 
matrix  can  now  be  determined  by  multiplying  the  upper  right  partition  of  Equation  (21) 
by  ;!r .  This  method  was  first  formulated  by  Van  Loan  in  1978  and  can  be  found  in  [Ref 
5]. 

B.  KALMAN  ALGORITHM 

Before  entering  the  Kalman  filter  loop,  an  initial  estimate,  xq  ,  and  its  error 

covariance,  Pq”  » “ust  chosen.  For  this  simulation,  these  initial  conditions  were  chosen  to 
be 

o' 

0 
0 
0 
0 
0 

(22) 

1  0  0  0  0  0 

0  1  0  0  0  0 

0  0  1  0  0  0 

0  0  0  1  0  0 

0  0  0  0  1  0 

0  0  0  0  0  1 

The superscript  will  represent  the  predicted  estimate  while  the  notation  denotes 
estimation.  The  discrete  Kalman  filter  is,  in  essence,  just  a  computer  algorithm  that 
derives  optimal  estimates  from  discrete  measurements.  Although  there  are  different 
foims  of  the  discrete  Kalman  filter,  the  most  fundamental  form  starts  with  the  Kalman 
gain  calculation,  which  is  given  by 


19 


G,  =  P[HliH,P,-Hl  +  R, )-‘  (23) 

The  value  of  this  gain  matrix  will  vary  with  each  time  step.  Next,  it  is  required  to  update 
the  estimate  using  star  sensor  data,  and  also  it  is  required  to  determine  the  accuracy  of 
this  new  estimate.  The  equations  are  given  as 

Xk  =  Xk  +  (xk  -f^kXk)  (24) 

P,={J-G,H,)P,-  (25)- 

Figure  5,  shown  below,  illustrates  the  recursive  nature  of  the  discrete  Kalman  filter.  A 
favorable  characteristic  of  any  recursive  filter  is  that  there  is  no  need  to  store  past 
measurements  [Ref.  6]. 


Figure  5;  Discrete  Kalman  Filter  Loop 
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Equation  (24)  is  the  actual  output  of  the  discrete  Kalman  filter.  It  estimates  both  attitude 
angles  and  attitude  rates  given  only  star  sensor  angle  information.  Not  only  does  it  derive 
rates,  but  it  also  mitigates  sensor  noise  effects.  Lastly,  it  is  necessary  to  project  ahead 
and  estimate  the  state  for  the  next  time  step.  The  predictive  equations  are  as  follows 

(26) 

P,;,  (27) 

It  is  interesting  to  note  that  in  Equation  (26),  the  deterministic  forcing  function  has  been 
included.  This  forcing  function  consists  of  known  reaction  wheel  moments,  which  can  be 
measured  by  the  reaction  wheel  motor  assembly.  If  this  deterministic  term  is  not 
included,  the  rate  estimator  is  unable  to  accurately  estimate  satellite-rates  near  perigee. 

For  the  purpose  of  analysis  and  proper  tuning,  it  is  helpful  to  look  at  the  time- 
varying  nature  of  both  the  Q  and  P  matrices  over  a  period  of  one  orbit.  Since  the  off- 
diagonal  elements  of  these  matrices  are  small,  only  the  diagonal  elements  will  be 
examined.  These  elements  are  shown  in  Figure  6  and  Figure  7. 
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Figure  7;  Error  Covariance  Elements 


If  Q  is  decreased,  the  filter  will  have  a  tendency  to  track  the  predicted  estimate. 
On  the  other  hand,  if  Q  is  increased,  the  filter  will  track  the  measurements.  It  is 
important  to  find  the  right  balance  because  if  Q  is  too  low,  then  the  filter  will  have  a  hard 
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time  tracking  when  or  if  the  satellite  maneuvers,  but  if  Q  is  too  high,  pointing  accuracy 
will  suffer  from  sensor  noise  effects.  The  results  of  the  Kahnan  filter  will  be  shown  in 
Chapter  VII. 
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VII.  PROPORTIONAL  PLUS  DERIVATIVE  CONTROLLER 


A  PD  controller  was  chosen  because  of  its  simplicity.  Although  this  type  of 
controller  does  not  reduce  or  correct  steady  state  errors,  the  gains  can  be  adjusted  to 
ensure  the  error  is  within  acceptable  limits.  A  separate  controller  will  be  assigned  to  each 
reaction  wheel. 

A.  CONTROL  LAWS 

Many  types  of  control  laws  are  available  which  can  conceivably  satisfy  this 
satellite's  pointing  requirements.  Some  common  control  laws  are  1)  proportional  2) 
proportional  plus  derivative  3)  proportional  plus  integral  plus  derivative  and  4)  optimal. 
Each  of  these  controllers  has  its  own  unique  characteristics;  however,  as  long  as  the 
controller  maintains  proper  spacecraft  attitude,  exotic  controllers  will  not  be  required.  In 
fact,  it  will  be  shown  that  the  gains  of  a  simple  PD  controller  can  be  adjusted  to  minimize  ' 
overshoot  and  settling  time.  Each  of  the  reaction  wheels  in  this  spacecraft  will  have  its 
own  PD  controller  and  they  are  represented  as 

hy  =  k^e  +  kyO  (28) 

These  control  laws  are  expressed  as  the  rate  of  change  of  reaction  wheel  angular 
momentmn,  or  reaction  wheel  torque,  and  they  are  part  of  the  feedback  loop.  As  can  be 
seen,  these  internal  torque  equations  are  a  function  of  the  measured  Euler  angles  and 
rates.  The  gain  constants  are  found  by  substituting  Equation  (28)  into  Equation  (55), 
which  is  located  in  Appendix  C.  If  the  resulting  set  of  equations  is  completely  de¬ 
coupled,  and  the  Laplace  transform  is  taken,  the  following  result  is  obtained 
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For  this  particular  ^alysis,  it  is  assiuned  that  the  orbital  angular  velocity  is  locally 
constant.  The  objective  is  to  determine. suitable  position  and  rate  feedbaclc gains  that  will 
increase  spacecraft  robustness.  The  nominal  characteristic  equation  for  any  second  order 
system  has  the  following  form 

A(s)  =  s^+26)„t^  +  co^  (30) 

The  natural  frequency  is  denoted  as  and  4"  is  the  damping  factor,  which  will  be 
chosen  to  be  one.  Each  of  the  denominators  in  Equation  (29)  will  be  equated  to  Equation 
(30).  Solving  for  the  coefficients,  the  result  is  two  equations  and  three  unknowns.  The 
third  equation  makes  use  of  the  final  value  theorem  [Ref  7],  and  it  is  given  by  the 
following 

/(oo)  =  lim  /(t)  =  lim  sF(s)  (31) 

r-^oo  S“>o 

The  pointing  requirements  for  this  satellite  require  a  steady  state  pointing  accuracy  of 
0.1°  about  each  axis.  By  applying  the  final  value  theorem  to  Equation  (29)  and  assuming 
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that  the  external  disturbance  torques  can  be  approximated  as  a  step  input,  position 
feedback  gains  can  be  determined  from  the  following  equations 


{-Ix  +  )¥ss  + 


(32) 


The  'ss'  subscript  denotes  steady  state  and  the  design  torques  represent  a  worst  case 
scenario.  It  can  be  seen  from  Equation  (32)  that  the  position  feedback  gains  are  not 
constant;  they  will  vary  as  a  function  of  orbital  position.  The  natural  frequency  for  roll, 
pitch  and  yaw  can  now  be  determined  by  taking  the  square  root  of  the  last  term  in  the 
denominator  in  Equation  (29).  Once  this  is  found,  the  velocity  feedback  gains  can  be 
calculated  from  the  following  expressions 

k^.=2CO„yly  (33) 

^vr  ~ 


In  a  similar  manner  to  the  position  feedback  gains,  the  velocity  feedback  gains  also  vary 
with  time.  Figure  8  and  Figure  9  each  depict  the  time  varying  nature  of  the  PD  gains  over 
one  orbit,  specifically  during  perigee.  As  expected  the  pitch  and  pitch  rate  gains  are 
much  higher  than  the  roll  and  yaw  gains. 
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Figure  8:  Controller  Gains 


Figure  9:  Controller  Rate  Gains 


In  the  above  analysis,  many  assumptions  were  made  in  order  to  achieve  suitable 
gains,  but  as  long  as  these  gains  minimize  overshoot  and  decrease  settling  time,  then  they 
are  acceptable.  For  clarification,  the  design  disturbance  torques  used  in  Equation  (32) 
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represent  the  aggregate,  worst-case  expected  torques,  both  internal  and  external.  If  the 
magnitude  of  this  torque  is  exceeded,  then  the  pointing  acctiracy  of  this  model  will  suffer. 
For  example,  the  worst-case  torque  about  the  y-axis  is  estimated  to  be  higher  than  the 
torques  about  the  other  two  axes  since  the  reaction  wheel  will  have  to  exert  a 
considerable  torque  at  perigee,  in  order  to  keep  the  spacecraft  nadir  pointing. 
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VIIL  RESULTS 


The  results  of  the  simulation  prove  that  it  is  possible  to  design  a  satellite  control 
system  without  rate  gyroscopes.  Figure  10  is  a  plot,  using  only  star  trackers,  of  satellite 
attitude  over  a  period  of  one  orbit.  Although  it  is  difficult  to  see  from  this  figure,  both 
actual  and  estimated  satellite  attitude  about  all  three  axes  have  been  plotted. 


In  order  to  analyze  Figine  10,  a  close-in  look  at  a  small  portion  of  each  curve  is  shown  in 
Figure  1 1,  Figure  12,  and  Figure  13.  If  the  star  sensors  were  ideal,  the  measurements 
(triangles)  would  all  fall  on  the  actual  attitude,  but  since  there  is  noise,  they  are  randomly 
dispersed.  The  non-measurements  in  Figure  1 1  occur  when  the  x-axis  star  sensor  is 
selected;  roll  angles  can  not  be  sensed,  as  a  result,  they  are  assigned  a  value  of  zero.  The 
Kalman  filter  makes  a  prediction  in  this  case.  It  can  be  seen  from  the  figures  below  that 
the  Kalman  filter  cuts  through  the  noise  and  tracks  the  attitude  effectively.  As  alluded  to 
earlier,  if  Q  is  decreased,  the  estimates  will  be  weighted  in  favor  of  the  predicted  estimate 
and  if  Q  is  increased  the  estimates  will  be  weighted  in  favor  of  the  measurements. 
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Figure  12:  Pitch  Response  with  Measurements 
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Figure  13;  Yaw  Response  with  Measurements 


Attitude  rates  are  estimated  from  star  tracker  data.  These  rates  traditionally  come 
from  rate  gyroscopes,  but  as  seen  in  Figure  14,  the  estimated  rates  are  very  accurate. 


Figure  14:  Attitude  Rates 
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Similar  to  Figure  10,  two  curves  are  actually  plotted  in  Figure  14.  Figure  15  is  a  close-in 
look  that  shows  that  the  estimate  is  accurate  to  within  10'^  rad/s.  No  measurements  are 
shown  since  star  sensors  can  only  measure  angles,  not  rates. 


Figure  15:  Actual  and  Estimate  Attitude  Rates 

The  output  of  the  Kalman  filter  is  fed  into  the  PD  controllers.  The  torque  applied 
to  the  reaction  wheels  over  a  period  of  one  orbit  is  shown  in  Figure  16.  As  expected, 
considerable  torque  is  applied  to  the  pitch  wheel  at  perigee.  The  angular  momentum  of 
the  pitch  wheel  at  perigee  is  therefore  also  high;  this  can  be  seen  in  Figme  17. 
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Figure  17:  Reaction  Wheel  Momentum 


If  there  is  an  attitude  error  about  any  axis,  internal  torques  will  arise  due  to  the  cross 
coupling  of  reaction  wheel  angular  momentum  components.  That  is  why,  in  Figure  16, 
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the  magnitude  of  the  torque  spikes  in  both  roll  and  yaw  at  perigee.  The  reaction  wheels 
work  against  each  other  until  the  satellite  achieves  the  proper  attitude. 
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IX.  SUMMARY  AND  CONCLUSION 


A.  SUMMARY 

In  summary,  the  equations  of  motion  that  were  derived  in  Appendix  A  were 
transformed  into  a  state  space  equation.  This  state  space  equation  was  discretized 
resulting  in  a  difference  equation.  This  difference  equation,  the  plant  model,  consisted  of 
a  state  transition  matrix  and  a  deterministic  control  matrix.  Star  trackers  provided  two- 
axis  measurements  to  the  Kalman  filter  at  a  rate  of  0.1  Hz.  The  output  of  the  filter  was 
fed  into  three  PD  controllers,  which  counteracted  disturbance  torques. 

B.  CONCLUSION 

From  the  previous  chapter,  it  was  proven  that  a  discrete  Kalman  filter  is  effective 
in  the  estimation  of  body  rates  firom  noisy  sensor  data.  The  results  show  that  rates  can  be 
estimated  to  within  10’’  r/s,  which  is  as  good  as  any  gyroscope.  These  results,  however, 
are  based  on  small  angle  approximations.  The  next  step  is  to  develop  a  quaternion  for  the 
large  angle  acquisition  phase  of  the  spacecraft.  In  order  to  handle  the  non-linear  nature  of 
the  quaternion,  an  extended  Kalman  filter  will  have  to  be  implemented. 

In  this  simulation,  everything  was  calculated  in  ten  second  intervals.  An 
additional  loop  needs  to  be  included  in  the  control  flow  diagram  that  will  speed  up  the 
rate  updates.  A  star  does  not  have  to  be  identified  in  order  to  calculate  rates,  it  only  needs 
to  be  identified  for  position  updates.  Fiuthermore,  processing  delays  were  not  included 
in  this  analysis.  The  satellite  was  modeled  as  a  rigid  body.  Further  studies  will  need  to 
study  the  dynamics  of  the  solar  arrays,  antennas  and  other  appendages. 

,  The  technology  for  rate  estimation,  using  only  star  trackers  for  attitude  updates, 
has  reached  the  level  where  it  is  more  feasible  than  using  both  gyroscopes  and  star 
trackers  for  attitude  determination. 
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APPENDIX  A:  KINEMATICS 


Three  reference  frames  will  be  used  in  the  derivation  of  equations  of  motion  1) 
Inertial  2)  Orbital  and  3)  Body.  Transformation  between  coordinate  systems  will  be  done 
using  direction  cosine  matrices  (DCM).  These  matrices  are  given  by 
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0 
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CXf/ 


Civ^)  = 


-sy/ 


0 


sy/  0 
cy/  0 
0  1 


The  orbital  reference  frame  is  oriented  such  that  the  x-axis  points  in  the  direction  of  the 
velocity  vector,  the  z-axis  points  towards  the  center  of  the  Earth  and  the  y-axis  completes 
the  right-hand  set.  It  is  desired  to  keep  the  body  frame  aligned  with  the  orbital  frame. 
The  transformation  from  the  orbital  reference  frame  to  the  body  frame  is  given  by  the 
following  3-2-1  transformation 


b 


c° 


=  c{Y)C{e)C{<i>)  = 


c6cy/ 

cOsy/ 

-s6 

-c(psy/  ->rs^Ocy/ 

c^y/  ^s^Gsy/ 

S(t)c9 
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(35) 


The  orbital  reference  frame  rotates  at  a  rate  of  with  respect  to  the  inertial  frame,  or 


'<5°  =  -002 


(36) 
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In  order  to  perform  angular  momentum  calculations,  it  is  required  to  express  the  inertial 
angular  velocity  in  body  coordinates.  The  inertial  angular  velocity  is  represented  by 

'■^*='■©‘’+‘’5*  (37) 

The  angular  velocity  of  the  orbital  frame  with  respect  to  the  inertial  frame,  expressed  in 
body  coordinates  is 

‘31  =-‘>C°Qo2  (38) 

The  angular  velocity  of  the  body  frame  with  respect  to  the  orbital  frame,  expressed  in 
body  coordinates  is 

=  #1  +  C((Z))C(0)<^2  +*C  >03  (39) 

The  ^2  unit  vector  belongs  to  an  intermediate  reference  frame.  If  Equation  (38)  and 
Equation  (39)  are  substituted  into  Equation  (37),  the  following  result  is  obtained 

‘3‘’  y/%  +{6-  a)b2  +W  +  (40) 

Equation  (40)  is  a  simplified  expression  where  small  angle  approximations  were  used  and 
second  order  terms  were  neglected. 
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APPENDIX  B:  GRAVITY  GRADIENT  TORQUES 

In  a  Molnyia  orbit,  gravity  gradient  moments  will  be  greatest  at  perigee.  The 
gravity  gradient  torque  is  given  by  [Ref.  1] 


=  1 


rxagdm 


The  gravitational  acceleration  is 


(41) 


a 


g 


=  -GM^ 


R  +  r 

1  3 

R  +  r 


(42) 


R  is  the  distance  to  the  center  of  mass  of  the  satellite  measured  from  the  center  of  the 
Earth  and  it  is  given  by 


(43) 

Equation  (43),  expressed  in  body  coordinates  is 

(44) 


For  now,  will  be  written  as 


R,,=Xb,+Yb^+Zb^  '  (45) 

Taking  the  cross  product 

rxR  =  {yZ  -  zY)b^  +  (zX  -  xZ)&2  +  {xY  -  yX)b^  (46) 

From  the  binomial  theorem,  the  following  expression  is  obtained 
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(47) 


I-  _i-3  1  ^xX  +  yV  +  zZ 

=-7-3 - ^ - 

It  can  be  shown  that  the  orbital  angular  velocity  is  just 

(48) 

Equation  (46),  Equation  (47),  and  Equation  (48)  can  be  substituted  into  Equation  (41)  to 
get  the  following  expression 

r^^=-3Q2[^(/,-/,)  +  #^  +  /^]  (49) 

These  three  equations  were  derived  using  small  angle  approximations  and  neglecting 
second  order  terms. 
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APPENDIX  C:  DERIVATION  OF  EQUATIONS  OF  MOTION 

When  determining  the  attitude  of  a  satellite,  it  is  helpful  to  translate  everything 
into  the  body  coordinate  system  since  on-board  sensors  will  detect  errors  with  respect  to 
the  body  frame.  Transformations  are  done  by  a  variety  of  techniques,  but  the  most 
elementary  method  is  known  as  the  direction  cosine  matrix.  From  Appendix  A,  it  was 
shown  that 

=  +((9-q)&2  +{\i/-D.0)^  (50) 


This  result  was  obtained  using  small  angle  approximations  where  the  symbol  ^ 
represents  the  error  in  roll,  6  represents  the  error  in  pitch,  and  y/  represents  the  error  in 
yaw.  The  total  spacecraft  angular  momentum  can  be  separated  into  two  vectors:  1) 
angular  momentum  of  the  spacecraft  body  and  2)  angular  momentum  of  the  reaction 
wheels,  and  it  is  given  by  the  following  expression 

H  =  Hi,+H^  (51) 

Assuming  cross  products  of  inertia  are  negligible,  the  following  expression  is  obtained 

(52) 

It  is  important  to  note  that  when  calculating  the  angular  momentum  of  the  satellite  about 
its  center  of  mass,  inertial  angular  rates  must  be  used  rather  than  body  rates.  Substituting 
Equation  (50)  and  Equation  (52)  into  Equation  (51),  total  spacecraft  angular  momentum 
is  found  to  be 

^  +  K  K  +  (lyS  -  +  hy  '^2  +{hy'  +  h^<l>  +  K  %  (53) 

The  following  relation  will  be  used  to  determine  the  Euler  moment  equations  [Ref  8] 
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(54) 


—  H  =  —  H+'cb^’xH 
dt  dt 

If  second  order  terms  are  neglected  and  gravity  gradient  moments  derived  in  Appendix  B 
are  incorporated,  it  can  be  shown  that  the  Euler  equations  for  this  spacecraft  are  just 

Ty=ly9  +  'iO}{jj^-l2)d  +  h^y/  +  Qh^\f/  +  Oh^^-h^^-IyCl  +  hy  (55)  - 

=  I  ^yjr  +  Q}{^I  /fl  y\ -Q.hytff +Q.h^  +q{Ix -ly  +l2)^-hj,9  +  hy^+I  ^tl(j>  +  h^ 

These  equations  completely  describe  the  motion  of  the  spacecraft  when  subject  to 
external  disturbance  torques.  The  rate  of  change  of  angular  momentum  of  each  reaction 
wheel  will  be  used  to  coimteract  the  disturbance  moments,  thereby  maintaining  the 
required  pointing  accuracy.  Solving  for  the  Euler  angles,  however,  is  no  trivial  task;  all 
three  differential  equations  are  second  order  and  coupled  together.  If  the  cross  products 
of  inertia  are  not  negligible,  the  equations  of  motion  become 

Tti  =T^  +{9-Cl-30}9)I^  +{a^y/  +  W  +  ^<l>Vxz  +(-2Q^-2Q^)/^_ 

Ty,  =  Ty+{-2Q.ii/  +  2Q}^+'(^-tly/)I^+2,0?I^+(2^-D.'^y/  +  ¥+^<l>)^yz  (56) 


r^l  =T^  +(2Q0-q2)/^  +(,9-tl-2Q?-9)Iy^ 
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APPENDIX  D:  MATLAB  CODE 


%%%%%%%%%%%%%%%%MAIN  PROGRAM  %%%%%%%%%%%%%%%%% 
% 

%  This  code  simulates  a  3-axis  stabilized  spacecraft  in  a  Molnyia  orbit.  The 
%  satellite  is  designed  to  be  nadir  pointing  to  within  0.1  degrees  about  each 
%  axis.  The  simulation  starts  with  the  satellite  at  perigee  and  progresses  in 
%  discrete,  ten  second  time  intervals  for  an  entire  orbit.  Three  orthogonal 
%  star  sensors,  each  aligned  with  the  body  axes,  sense  attitude  errors  caused 
%  by  various  disturbance  torques.  Due  to  limited  onboard  processing 
%  capabilities,  only  one  star  sensor  can  make  an  observation  at  each  time  step; 

%  this  star  sensor  is  selected  at  random.  These  measurements,  however,  are 
%  corrupted  by  additive  white  noise.  A  six-state  discrete  Kalman  filter  is 
%  used  to  both  diminish  sensor  noise  effects  and  estimate  rates.  The  data  fi-om 
%  the  Kalman  filter  is  then  fed  back  to  three  independent,  proportional  plus 
%  derivative  controllers  thus  completing  the  control  loop.  This  code  calls  three 
%  functions:  ssorbit,  ssgains,  and  ssmatrix. 

% 

% 


% 

Ix=25000; 

Iy^25000; 

Iz=15000; 

% 

mu=398601; 

rp=7378.15; 

ra=42164.17; 

a=(rp+ra)/2; 

e=(ra-rp)/(ra+rp); 

p=a*(l-e^2); 

% 

Tspx=le-5; 


%  Moments  of  inertia 

%  Gravitational  constant 
%  Radius  of  perigee 
%  Radius  of  apogee 
%  Semi-major  axis 
%  Eccentricity 
%  Semi-latus  rectum 
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Tspy=le-5; 

Tspz=le-5; 

% 

rO=rp; 

v0=sqrt(2*mu/r0-mu/a); 

omegaO=vO/rO; 

hyO=-omegaO*Iy; 

% 

t0=0; 
dt=10; 
t5=50000; 
tspan=[tO:dt:tf]; 
yo=[rO  0  0  omegaO]; 
options  =  odeset('RelTor,le-6); 
[t,y]=ode45('ssorbit',tspan,yo,options,e,mu,p); 
% 

s=size(t); 

kmax=s(l,l)+l; 

% 

x=zeros(6,kmax); 
xkk=zeros(6,kmax); 
xkkm  1  =zeros(6,kmax) ; . 
z=zeros(2,kfflax); 
zx=zeros(  1  ,kmax) ; 
zy=zeros(l  ,kmax); 
zz=zeros(  1  ,kmax) ; 
h=zeros(3  ,kinax) ; 

T  c=zeros(3  ,kmax) ; 
tiine=zeros(  1  ,kmax); 

P=le-12*eye(6); 
h(:,l)=[0hy0  0]'; 


%  Solar  pressure  moments 


%  Radius  at  t=0  (perigee) 

%  Velocity  at  t=0 
%  Orbital  angular  rate  at  t=0 
%  RW  angular  momentum  at  t=0 


%  Time  span 
%  Initial  conditions 
%  Accuracy  of  convergence 
%  Integration 


%  Plant  array 
%  Estimation  array 
%  Prediction  array 
%  Measurement  array 


%  RW  angular  momentum  array 
%  RW  torque  array 
%  Sampling  time  array 
%  Initial  error  covariance 
%  Initial  RW  angular  momentum 
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% 

% 

% 

fori=l:kmax-l 

% 


% 

% 


[F,k,Wo]=ssgains(y(i,:)',h(:,i),Ix,Iy,Iz); 

[A3,Wdot]=ssmatrix(y(i,:)',h(:,i),mu,p,e,Ix,Iy,Iz); 

Aaug=A-B*F; 

[phik,delk]=c2d(Aaug,B5dt); 

ud=[(Tspx+Wo*h(3,i))/Ix  Tspy/Iy+Wdot ... 

(Tspz-Wo*h(l,i))/Iz]'; 

W=le-14*diag([.01  1  .01]); 

Aq=[-Aaug  B*W*B';zeros(6)  Aaug']*dt; 

Bq=expm(Aq); 

phiq=Bq(7:12,7:12)'; 

Q=phiq*Bq(l:6,7:12); 

c=rand; 


%  PD  controller  gains 
%  Plant  and  control  matrices 
%  Augmented  plant  matrix 
%  Discrete  A  and  B  matrices 

%  Disturbance  torques 
%  Power  spectral  density 


%  Plant  noise  covariance 
%  Random  number  generator 


% 


if  c<=  3,333 
N=2e-5; 

R=N'^2*eye(2); 

H=[0  0  1  0  0  0;0  0  0  0  1  0]; 

z(:,i)-H*x(;,i)+N*randn*ones(2,l); 

zy(i)=z(l,i); 

zz(i)=z(2,i); 

elseif  c>.3333  &  c<=.6666 


%  1  STAR  SENSOR/TIME  STEP 
%  Roll  sensor  noise 
%  Roll  sensor  covariance 
%  Pitch  &  yaw  observations 
%  Measurement  with  sensor  noise 
%  Observation  in  pitch 
%  Observation  in  yaw 


N=2e-5; 

R-N'^2*eye(2); 

H=[l  0  0  0  0  0;0  0  0  0  1  0]; 


%  Pitch  sensor  noise 
%  Pitch  sensor  covariance 
%  Roll  &  yaw  observations 
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z(:,i)=H*x(:,i)+N*randn*ones(2, 1 ); 
2x(i)=z(l,i); 
zz(i)=z(2,i); 
else 

N=2e-5; 

R=N^2*eye(2); 

H=[l  0  0  0  0  0;0  0  1  0  0  0]; 
z(:,i)=H*x(:,i)+N*randn*ones(2, 1 ); 

zx(i)=z(l,i); 

zy(i)=z(2,i); 

end 

% 

%  Nx=2e-5; 

%  Ny=2e-5; 

%  Nz=2e-5; 

%  Rx=4e-10*eye(2); 

%  Ry=4e-10*eye(2); 

%  Rz=4e-10*eye(2); 

%  Hx=[00  1  0  00;0  0  00  1  0]; 

%  Hy=[l  0  0  0  0  0;0  0  0  0  1  0]; 

%  Hz=[l  0  0  0  0  0;0  0  1  0  0  0]; 

%  x(:,i+l)=phik*x(:,i)+delk*ud; 

% 

% 

% 

% 

% 

% 

% 

% 

% 


%  Measurement  with  sensor  noise 
%  Observation  in  roll 
%  Observaton  in  yaw 

%  Yaw  sensor  noise 
%  Yaw  sensor  covariance 
%  Roll  &  pitch  observations 
%  Measurement  with  sensor  noise 
%  Observation  in  roll 
%  Observation  in  pitch 

%  3  STAR  SENSOR/TIMESTEP 

%  Nominal  star  sensor  noise 

%  Sensor  noise  covariance 

%  x-axis  star  sensor 
%  y-axis  star  sensor 
%  z-axis  star  sensor 
%  Plant 


Gx=P*Hx'*inv(Hx*P*Hx'+Rx);  %  Initial  Kahnan  gain 

zx(:,i)=Hx*x(:,i)+Nx*randn*ones(2,l);  %  Noisy  y  and  z  measurements 

xkkl=xkkml(:,i)+Gx*(zx(;,i)-Hx*xkkml(:,i));  %  Initial  estimate 
p  1  =(eye(6)-Gx*Hx)*P;  %  Initial  error  covariance 


Gy=Pl  *Hy’*inv(Hy*Pl  *Hy'+Ry); 

zy(:,i)=Hy*x(:,i)+Ny*randn*ones(2,l); 

xkk2=xkkl+Gy*(zy(:,i)-Hy*xkkl); 


%  Updated  Kahnan  gain 
%  Noisy  X  and  z  measurements 
%  Updated  estimate 
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%  P2=(eye(6)-Gy*Hy)*Pl; 

% 

%  Gz=P2*Hz’*inv(Hz*P2*Hz’+Rz); 

%  Updated  error  covariance 

%  Final  Kalman  gain 

%  zz(:,i)=Hz*x(:,i)+Nz*randn*ones(2,l); 

%  Noisy  X  and  y  measurements 

%  xkk(:,i)=xkk2+Gz*(zz(:,i)-Hz*xkk2); 

%  Final  estimate 

%  Pk=(eye(6)-Gz*Hz)*P2; 

%  Final  error  covariance 

% 

x(:,i+l)=phik*x(:,i)+delk*ud; 

%  Plant 

G=P*H'*inv(H*P*H'+R); 

%  Kalman  gain 

xkk(:,i)=^kkml  (:,i)+G*(z(:,i)-H*xkkml 

%  Current  estimate 

% 

if  zx(i)=0 

%  No  roll  measurement  case 

xkk(l  :2,i)=xkkml(l  :2,i); 

%  Roll  &  roll  rate  prediction 

elseif  zy(i)=0  ' 

%  No  pitch  measurement  case 

xkk(3 :4,i)=xkkml  (3 :4,i); 

%  Pitch  &  pitch  rate  prediction 

elseif  zz(i)==0 

%  No  yaw  measurement  case 

xkk(5 :6,i)=xkkml  (5 :6,i); 

%  Yaw  &  yaw  rate  prediction 

end 

% 

Pk=(eye(6)-G*H)*P; 

%  Current  error  covariance 

xkkm  1  (:  ,i+ 1  )=phik*xkk( :  ,i)+delk*ud; 

%  Future  estimate 

P=phik*Pk*phik'+Q; 

%  Future  error  covariance 

Tc(:,i)=-k*xkk(:,i); 

%  Control  torques  (using  xkk) 

h(:,i+l)=h(:,i)+Tc(:,i)*dt; 

%  RW  angular  momentum 

aeig(:,i)=eig(A); 

%  Eigenvalues,  no  controller 

augeig(:,i)=eig(Aaug); 

%  Eigenvalues,  PD  controller 

% 

kx(i)=k(l,l); 

%  Roll  gain 

kvx(i)=k(l,2); 

%  Roll  rate  gain 

ky(i)=k(2,3); 

%  Pitch  gain 

kvy(i)=k(2,4); 

%  Pitch  rate  gain 
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%  Yaw  gain 
%  Yaw  rate  gain 


kz(i)=k(3,5); 
kvz(i)=k(3,6); 
qll(i)=Q(l,l); 

q33(i)=Q(3,3);  %  Elements  of  Q 

q55(i)=Q(5,5); 

pll(i)=P(l,l); 

p33(i)=P(3,3);  %  Elements  of  P 

p55(i)=P(5,5); 

time(i+l)=time(i)+dt;  %  Time  steps 

% 

% 

% 

end 

% 

% 

% 

% 

% 

% 

%%%%%%%%%%%%%%%%%%  RESULTS  %%%%%%%%%%%%%%%%%%% 

r=1001:1101; 

tl=time(r); 

t2=time(l  :kmax-l); 

% 

% 

% 

figure(l) 

subplot(221) 

plot(time,x(  1  ,:)*  1 80/pi) 

title('Roll  Response  using  Ideal  Gyros/Sensors') 

xlabel('Time  (sec),  Orbit=3.88(10M)') 


50 


ylabel('Roll  (deg)') 
axis  tight 
grid; 

% 

subplot(222) 
plot(time,x(3,:)*  1 80/pi) 

title('Pitch  Response  using  Ideal  Gyros/Sensors') 
xlabel('Time  (sec),  Orbit=3.88(10M)') 
ylabel(’Pitch  (deg)’) 
axis  tight 
grid; 

% 

subplot(223) 
plot(time,x(5,:)*  1 80/pi) 

title('Yaw  Response  using  Ideal  Gyros/Sensors') 
xlabel('Time  (sec),  Orbit=3.88(10M)') 
ylabel('Yaw  (deg)') 
axis  tight 
grid; 

% 

% 

% 

figure(2) 

subplot(221) 

plot(t,y(:,l)) 

title('Orbit  Radius’) 

xlabelCTime  (sec),  Orbit=3.88(10M)') 

ylabel('Radius  (km)') 

grid; 

% 

subplot(222) 
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plot(t,y(:,3)*  180/pi) 

title('True  Anomaly') 

xlabel('T ime  (sec),  Orbit=3 .88(1  OM)') 

ylabel('True  Anomaly  (deg)') 

grid; 

% 

subplot(223) 

plot(t,y(:,4)) 

title('Orbital  Angular  Velocity') 
xlabel('Time  (sec),  Orbit=3.88(10M)') 
ylabel('Wo  (rad/s)') 
grid; 

% 

% 

% 

figure(3) 

subplot(221) 

plot(time,Tc(l,:)) 

title('Roll  Control  Torque') 

xlabel('Time  (sec),  Orbit=3.88(10M)') 

ylabel('Torque  (N-m)') 

axis  tight 

grid; 

% 

subplot(222) 

plot(time,Tc(2,:)) 

title('Pitch  Control  Torque') 

xlabel('Time  (sec),  Orbit=3.88(10M)') 

ylabel('Torque  (N-m)') 

axis  tight 

grid; 
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% 

subplot(223) 

plot(time,Tc(3,:)) 

title('Yaw  Control  Torque') 

xlabel('Time  (sec),  Orbit=3.88(10M)') 

ylabel('Torque  (N-m)') 

axis  tight 

grid; 

% 

% 

% 

figure(4) 
subplot(221) 
plot(time4i(l ,:)) 

title('Reaction  Wheel  Momentum,  hx') 
xlabel('Time  (sec),  Orbit=3.88(10M)') 
ylabel('hx  (Nms)') 
axis  tight 
grid; 

% 

subplot(222) 

plot(time,h(2,:)) 

title('Reaction  Wheel  Momentum,  hy') 
xlabel('Time  (sec),  Orbit=3.88(10M)') 
ylabel('hy  (Nms)') 
axis  tight 
grid; 

% 

subplot(223) 

plot(time,h(3,:)) 

title('Reaction  Wheel  Momentum,  hz') 
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xlabel('Time  (sec),  Orbit=3.88(iOM)') 
ylabel('hz  (Nms)') 
axis  tight 
grid; 

% 

% 

% 

figure(5) 
subplot(221) 
plot(t2,kx) 
title('Roll  gain') 

xlabel(’Time  (sec),  Orbit=3.88(10M)') 
ylabel('kx  (N-m/rad)') 
axis  tight 
grid; 

% 

subplot(222) 
plot(t2,ky) 
title('Pitch  gain') 

xlabel('Time  (sec),  Orbit=3.88(10M)') 
ylabel('ky  (N-m/rad)') 
axis  tight 
grid; 

% 

subplot(223) 
plot(t2,kz) 
title('Yaw  gain') 

xlabel('Time  (sec),  Orbit=3.88(10'^4)') 
ylabelCkz  (N-m/rad)') 
axis  tight 
grid; 


% 

% 

% 

figure(6) 

subplot(221) 

plot(t2,kvx) 

title('Roll  rate  gain') 

xlabel('Time  (sec),  Orbit=3.88(l OM)') 

ylabel('kvx  (N-m-s/rad)') 

axis  tight 

grid; 

% 

subplot(222) 

plot(t2,kvy) 

title(’Pitch  rate  gain') 

xlabel('Time  (sec),  Orbit=3.88(10M)') 

ylabel('kvy  (N-m-s/rad)') 

axis  tight 

grid; 

% 

subplot(223) 

plot(t2,kvz) 

title('Yaw  rate  gain') 

xlabel('Time  (sec),  Orbit=3.88(10M)') 

ylabel('kvz  (N-m-s/rad)') 

axis  tight 

grid; 

% 

% 

% 

figure(7) 
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subplot(221) 

plot(time,x(l  ,:)*  1 80/pi, time, xkk(l  ,:)*  1 80/pi) 
title('True  &  Estimated  Roll  Response') 
xlabel('Time  (sec),  Orbit=3.88(10M)') 
ylabelCRoll  (deg)') 
axis  tight 
grid; 

% 

subplot(222) 

plot(time,x(3,:)*  1 80/pi,'-',time,xkk(3,:)*  1 80/pi) 
title('True  &  Estimated  Pitch  Response') 
xlabel('Time  (sec),  Orbit=3.«8(10M)') 
ylabel('Pitch  (deg)') 
axis  tight 
grid; 

% 

subplot(223) 

plot(time,x(5,:)*  1 80/pi,'-',time,xkk(5,:)*  1 80/pi) 
title('True  &  Estimated  Yaw  Response') 
xlabel('Time  (sec),  Orbit=3.88(10M)') 
ylabel('Yaw  (deg)') 
axis  tight 
grid; 

% 

•% 

% 

figure(8) 

subplot(221) 

plot(time,x(2,:),'-',time,xkk(2,:)) 
title('Trae  &  Estimated  Roll  Rate') 
xlabel('Time  (sec),  Orbit=3.88(10M)') 
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ylabel('Roll  Rate  (r/s)') 

axis  tight 

grid; 

% 

subplot(222) 

plot(time,x(4, time, xkk(4, :)) 
title('True  &  Estimated  Pitch  Rate') 
xlabel(’Time  (sec),  Orbit=3.88(10My) 
ylabel('Pitch  Rate  (r/s)’) 
axis  tight 
grid; 

% 

subplot(223) 

plot(time,x(6,:),'-',time,xkk(6,:)) 
title('True  &  Estimated  Yaw  Rate') 
xlabel('Time  (sec),  Orbit=3.88(10M)') 
ylabel('Yaw  Rate  (r/s)') 
axis  tight 
grid; 

% 

% 

% 

figure(9) 

subplot(221) 

plot(tl  ,x(l  ,r),tl  ,xkk(l  ,r),’.-') 
title('Snapshot  of  Roll  Response') 
xlabel('Time  (sec),  Orbit=3.88(10M)') 
ylabel('Roll  (deg)') 
axis([min(tl)  max(tl)  -3e-5  6e-5]); 
grid; 

% 


57 


subpiot(222) 

plot(tl  ,x(3,r),tl  ,xkk(3,r),'.-') 
title('Snapshot  of  Pitch  Response') 
xlabel(’Time  (sec),  Orbit=3.88(10M)') 
ylabel('Pitch  (deg)') 
axis([min(tl)  niax(tl)  -3e-5  3e-5]); 
grid; 

% 

subplot(223) 

plot(tl,x(5,r),tl,xkk(5,r),'.-') 
title('Snapshot  of  Yaw  Response') 
xlabel('Time  (sec),  Orbit=3.88(10M)') 
ylabel('Yaw  (deg)') 
axis([min(tl)  max(tl)  le-5  8e-5]); 
grid; 

% 

% 

% 

figiire(lO) 

subplot(221) 

plot(tl  ,x(2,r),tl  ,xkk(2,r),'.-') 
title('Snapshot  of  Roll  Rate') 
xlabel('Time  (sec),  Orbit=3.88(10M)') 
ylabel('Roll  Rate  (r/s)') 
axis([inin(tl)  max(tl)  -4e-7  4e-7]); 
grid; 

% 

subplot(222) 

plot(tl  ,x(4,r),tl  ,xkk(4,r),'.-') 
title('Snapshot  of  Pitch  Rate') 
xlabel('Time  (sec),  Orbit=3.88(10M)’) 


ylabel('Pitch  Rate  (r/s)') 
axis([inin(tl)  max(tl)  -4e-7  4e-7]); 
grid; 

% 

subplot(223) 

plot(tl,x(6,r),tl,xkk(6,r),'.-') 
title('Snapshdt  of  Yaw  Rate’) 
xlabel('Time  (sec),  Orbit=3.88(10M)') 
ylabel('Yaw  Rate  (r/s)') 
axis([min(tl)  max(tl) -4e-7  4e-7]); 
grid; 

% 

% 

% 

figure(ll) 

subplot(221) 

plot(t2,qll) 

title('qll') 

xlabel('Time  (sec),  Orbit=3.88(10^4)') 

axis  tight 

grid; 

% 

subplot(222) 

plot(t2,q33) 

title('q33') 

xlabel('Time  (sec),  Orbit=3.88(10M)') 
axis([min(t2)  max(t2)  2.0240e-12  2.0242e- 
grid; 

% 

subplot(223) 

plot(t2,q55) 


titleCq55’) 

xlabel(’Time  (sec),  Orbit=3.88(10M)’) 

axis  tight 

grid; 

% 

% 

% 

figure(12) 

subplot(221) 

plot(t2,pll) 

title('pir) 

xlabel('Time  (sec),  Orbit=3.88(10M)') 

axis  tight 

grid; 

% 

subplot(222) 

plot(t2,p33) 

title('p33') 

xlabel('Time  (sec),  Orbit=3.88(10M)') 

axis  tight 

grid; 

% 

subplot(223) 

plot(t2,p55) 

title('p55') 

xlabelCTime  (sec),  Orbit=3.88(10M)') 

axis  tight 

grid; 

% 

% 

% 
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figure(13) 

plot(tl  ,x(  1  ,r),tl  ,xkk(l  ,zx(r),'^') 
title('Roll  Response  Snapshot  with  Observations') 
xlabelCTime  (sec),  Orbit=3.88(10M)') 
ylabel('Roll  (deg)') 
axis  tight 
grid 
% 

% 

% 

figure(14) 

plot(tl  ,x(3,r),tl  ,xkk(3,r),'.-',tl  ,zy(r),'^') 

title('Pitch  Response  Snapshot  with  Observations'), 

xlabel('Time  (sec),  Orbit=3.88(10M)') 

ylabel('Pitch  (deg)') 

axis  tight 

grid 

% 

% 

% 

figure(15) 

plot(tl  ,x(5,r),tl  ,xkk(5,r),'.-',tl  ,zz(r),''^') 

title('Yaw  Response  Snapshot  with  Observations') 

xlabel('Time  (sec),  Orbit=3.88(10M)') 

ylabel('Yaw  (deg)') 

axis  tight 

grid 

% 

% 

% 

figure(16) 
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subplot(21 1) 
plot(aeig,'.') 

title('Root  Locus,  No  Controller') 

xlabel('Rear) 

ylabel('Imaginary') 

grid 

% 

subplot(212) 

plot(augeig,'.') 

title('Root  Locus,  PD  Controller')  • 

xlabel('Rear) 

ylabel('Iniaginary') 

axis([-.035  .035  -3e-3  3e-3]) 

grid 

%%%%%%%%%%%%%%%%%%% END  MAIN  %%%%%%%%%%%%%%%%% 
% 

% 

% 

function  [F,k,Wo]=ssgains(y,h,Ix,Iy,Iz) 

%  This  function  computes  non-optimal  gains  for  each  PD  controller.  Both  the 
%  position  and  rate.gains  will  be  time  varying.  In  order  to  keep  the  satellite 
%  nadir  pointing  at  perigee,  it  is  expected  that  the  pitch  gains  will  be  much 
%  higher  than  the  roll  and  yaw  gains.  The  F  matrix  is  used,  in  part,  to  augment 
%  the  A  matrix.  The  resulting  augmented  plant  matrix  has  all  six  eigenvalues  in 
%  the  left  hand  plane,  which  is  a  requirement  for  system  stability.  The  k  matrix 
%  is  used  to  calculate  the  reaction  wheel  control  torques. 

% 

% 

% 

Wo=y(4,:);  %  Orbital  angular  velocity 

hy=h(2,:);  %  Angular  momentum  of  y  RW 
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%  Design  torques 


% 

Tx=5e-4; 

Ty=5e-2; 

Tz=5e-4; 

% 

ssphi=  l*pi/180; 

sstheta=.l*pi/180;  %  Allowable  Steady  State  Errors 

sspsi=  l*pi/180; 

% 

kx=(Tx-4*Wo^2*(Ix-Iy)*ssphi+Wo*hy*ssphi)/ssphi; 

wnx=sqrt(4*Wo^2*(Iy-Iz)/Ix+kx/Ix);  %  Roll  gains  &  natural  freq. 

kvx=2*wnx*Ix; 

% 

ky=(Ty-3  *  W  o^2*  (Ix-Iz)*  sstheta)/sstheta; 

wny=sqrt(3 *  Wo^2*(Ix-Iz)/Iy+ky/Iy);  %  Pitch  gains  &  natural  freq. 

kvy=2*wny*Iy; 

% 

kz=(Tz-Wo'^2*(-Ix+Iy)*sspsi+Wo*hy*sspsi)/sspsi; 

wnz=sqrt(Wo^2*(-Ix+Iy)/Iz+kz/Iz);  %  Yaw  gains  &  natural  freq. 

kvz=2*wnz*Iz; 

% 

% 

% 

F=[kx/Ix  kvx/Ix  0  0  0  0;... 

0  0  ky/Iy  kvy/Iy  0  0;... 

0  0  0  0  kz/Iz  kvz/Iz];  %  Controller  gain  matrix 

% 

k=[kx  kvx  0  0  0  0;... 

0  0  ky  kvy  0  0;... 

0  0  0  0  kz  kvz];  %  Gain  matrix 

%%%%%%%%%%%%%  END  FUNCTION  SSGAIN  %%%%%%%%%%%%%%%% 
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Q. 

O 

% 

Q, 

'O 

function  ydot=ssorbit(t,y,FLAG,e,niu,p) 

%  This  function  solves  two  first  order  differential  equations  for  radius 
%  and  true  anomaly  using  a  Runge-Kutta  integration  scheme.  In  order  to 
%  have  orbital  angular  velocity  and  acceleration  available  for  each  star 
%  sensor  measurement,  the  differential  equations  were  solved  at  fixed, 

%  discrete  time  step.  The  duration  of  the  time  steps  is  ten  seconds, 

%  same  as  the  star  sensor  sampling  time. 


% 

% 

% 

r=y(l,:); 

%  Orbital  radius 

rdot=y(2,:); 

%  Rate  of  change  of  radius 

nu=y(3,:); 

%  True  anomaly 

Wo=y(4,:); 

%  Orbital  angular  velocity 

%  Output 
ydot(l,:)=rdot; 

ydot(2,:)=sqrt(mu/p)*e*cos(nu)*Wo; 

ydot(3,:)=Wo; 

ydot(4,:)=-sqrt(mu/p)/r^2*(r*e*sin(nu)*Wo... 

+(l+e*cos(nu))*sqrt(mu/p)*e*sin(nu)); 

%%%%%%%%%%%%%%%  END  FUNCTION  SSORBIT  %%%%%%%%%%%%% 
% 

%  •  ■ 

% 

function  [A,B,Wdot]=ssmatrix(y,h,mu,p,e,Ix,Iy,Iz) 

%  This  function  computes  the  system  matrices  for  the  state  space  equations. 

%  Both  the  A  and  B  matrices  are  time  varying  since  they  both  include 


64 


%  orbital  angular  velocity  and  orbital  angular  acceleration  terms.  It  is 
%  assumed  that  these  values  can  be  pre-calculated  and  stored  in  the  satellite 
%  computer.  According  to  engineers  at  the  Aerospace  corporation,  this  is  not 
%  an  unreasonable  assumption.  Each  time  a  star  sensor  observation  is  made, 
%  associated  with  that  measurement  is  a  dedicated  orbital  angular  velocity 
%  and  acceleration. 

% 


% 


% 


i=y(U0; 

nu=y(3,:); 

Wo-y(4,:); 

% 

hx=h(l,:); 

hy=h(2,:); 

hz=h(3,:); 

% 

Wdot=-sqrt(mxi/p)/r'^2*(r*e*sin(nu)*  Wo. . . 

+( 1  +e*cos(nu))*sqrt(mu/p)*e*sin(nu)); 
% 


%  Orbital  radius 
%  True  anomaly 
%  Orbital  angular  velocity 

%  RW  angular  momentum 


%  Orbital  angular  acceleration 


A=[0  1  0  0  0  0;... 

(-4*Wo^2*(Iy-Iz)+Wo*hy)/Ix  0  0 ... 

-hz/Ix  Wdot  (-Wo*(-Ix+Iy-Iz)+hy)/Ix;... 

0  0  0  1  0  0;... 

-Wo*hx/Iy  hz/Iy  -3*Wo^2*(Ix-Iz)/Iy  0  ... 

-Wo*hz/Iy  -hx/Iy;... 

0  0000  1;... 

-Wdot  (-Wo*(Ix-Iy+Iz)-hy)/Iz  0  hx/Iz ... 

(-Wo'^2*(-Ix+Iy)+Wo*hy)/Iz  0];  %  Plant  matrix 

B=[0  0  0;1  0  0;0  0  0;0  1  0;0  0  0;0  0  1];  %  Control  Matrix 

%%%%%%%%%%%%%  END  FUNCTION  SSMATRIX  %%%%%%%%%%%%%% 
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